import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    robot1_name_in_model = 'robot1'
    robot2_name_in_model = 'robot2'
    robot3_name_in_model = 'robot3'
    robot1_urdf_name = "originbot_gazebo1.urdf"
    robot2_urdf_name = "originbot_gazebo2.urdf"
    robot3_urdf_name = "originbot_gazebo3.urdf"
    package_name = 'originbot_description'
    
    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    robot1_urdf_model_path = os.path.join(pkg_share, f'urdf/{robot1_urdf_name}')
    robot2_urdf_model_path = os.path.join(pkg_share, f'urdf/{robot2_urdf_name}')
    robot3_urdf_model_path = os.path.join(pkg_share, f'urdf/{robot3_urdf_name}')
    gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')

    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

    # Launch the robot1
    robot1_spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot1_name_in_model,  '-file', robot1_urdf_model_path,'-x','0','-y','0'], 
        output='screen')
	
    # Start Robot1 State publisher
    start_robot1_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        namespace='robot1',
        arguments=[robot1_urdf_model_path]
    )
    
    # Launch the robot2
    robot2_spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot2_name_in_model,  '-file', robot2_urdf_model_path,'-x','0','-y','1'], 
        output='screen')
	
    # Start Robot2 State publisher
    start_robot2_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        namespace='robot2',
        arguments=[robot2_urdf_model_path]
    )


    # Launch the robot3
    robot3_spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot3_name_in_model,  '-file', robot3_urdf_model_path,'-x','1','-y','0'], 
        output='screen')
	
    # Start Robot3 State publisher
    start_robot3_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        namespace='robot3',
        arguments=[robot3_urdf_model_path]
    )

    ld.add_action(start_gazebo_cmd)
    ld.add_action(robot1_spawn_entity_cmd)
    ld.add_action(start_robot1_state_publisher_cmd)
    ld.add_action(robot2_spawn_entity_cmd)
    ld.add_action(start_robot2_state_publisher_cmd)
    ld.add_action(robot3_spawn_entity_cmd)
    ld.add_action(start_robot3_state_publisher_cmd)
    return ld
